#include <itkImage.h>
#include <itkImageFileReader.h>
#include <itkImageFileWriter.h>
#include <itkExtImageMoments.h>
#include "itkVectorImage.h"
#include "itkVector.h"
#include "itkVariableLengthVector.h"
#include "itkRigid2DTransform.h"
#include "itkSimilarity2DTransform.h"
#include "itkUnaryFunctorImageFilter.h"
#include "itkResampleImageFilter.h"
#include "vnl/vnl_math.h"
#include "itkRegionOfInterestImageFilter.h"
#include "itkMetaImageIO.h"
#include "itkImageIOBase.h"
#include "itkFlipImageFilter.h"

#define EPSILON 1e-6

const unsigned int Dim = 2;
typedef unsigned char                  TPixel;
typedef itk::Image< TPixel, Dim >      TImage;
typedef itk::ImageFileReader< TImage > TReader;
typedef itk::ImageFileWriter< TImage > TWriter;
typedef itkExt::ImageMoments< TImage > TImageMoments;
typedef itk::VectorImage<float, 2>  ImageType;

void drawLine(TImage* img, unsigned int x0,unsigned int y0,unsigned int x1,unsigned int y1)
{
    double dx, dy;
    double dxdy;
    double dydx;
    unsigned int x, y;
    double xx, yy;
    int color = 255;
    
    dx = (double)(x1-x0);
    dy = (double)(y1-y0);
    
 

    TImage::SizeType size = img -> GetRequestedRegion( ).GetSize( );
    if ( abs(dx) > EPSILON && abs(dy/dx) <= 1 && x1 > x0 ) {
        // Pendiente entre -1 y 1
        dydx = dy/dx;
        for ( x = x0, yy = (double)y0; x <= x1; x++ ) {
            y = (int)yy;
	    if ( x >= 0 && x < size[0]  &&
                 y >= 0 && y < size[1] ) {
                TImage::IndexType idx;
                idx[ 0 ] = x;
                idx[ 1 ] = y;

		img->SetPixel(idx, TPixel(color));
            }
            yy += dydx;
        }
      }
      else if (abs(dx) > EPSILON && abs(dy/dx) <= 1 && x1 < x0 ) {
        // Pendiente entre -1 y 1
        dydx = dy/dx;
        for ( x = x1, yy = (double)y1; x <= x0; x++ ) {
            y = (int)yy;
            if ( x >= 0 && x < size[0] &&
                 y >= 0 && y < size[1] ) {
                TImage::IndexType idx;
                idx[ 0 ] = x;
                idx[ 1 ] = y;

		img->SetPixel(idx, TPixel(color));
            }
            yy += dydx;
        }
      }
      else if ( abs(dy) > EPSILON && y1 > y0 ) {
        // Pendiente mayor a 1 o menor a -1
        dxdy = dx/dy;
        for ( y = y0, xx = (double)x0; y <= y1; y++ ) {
            x = (int)xx;
            if ( x >= 0 && x < size[0] &&
                 y >= 0 && y < size[1] ) {
                TImage::IndexType idx;
                idx[ 0 ] = x;
                idx[ 1 ] = y;

		img->SetPixel(idx, TPixel(color));
            }
            xx += dxdy;
        }
      }
      else if ( abs(dy) > EPSILON && y1 < y0 ) {
        // Pendiente mayor a 1 o menor a -1
        dxdy = dx/dy;
        for ( y = y1, xx = (double)x1; y <= y0; y++ ) {
            x = (int)xx;
            if ( x >= 0 && x < size[0] &&
                 y >= 0 && y < size[1] ) {
                TImage::IndexType idx;
                idx[ 0 ] = x;
                idx[ 1 ] = y;

		img->SetPixel(idx,TPixel(color));
            }
            xx += dxdy;
        }
      }
    ;

}

float calcularAnguloDeRotacion(float x, float y)
{
	float angulo = 0;
	angulo = atan(x / y);
	std::cout << x / y << std::endl;
	std::cout << angulo << std::endl;
	return angulo;
}

int main( int argc, char* argv[ ] )
{
  if( argc < 3 )
  {
    std::cerr
      << "Usage: " << argv[ 0 ]
      << " inputImageFile" << std::endl;
    return( 1 );

  } // fi
  std::string inputImageFile = argv[ 1 ];
  std::string outputImageFile = argv[ 2 ];
  
  TReader::Pointer reader = TReader::New( );
  reader->SetFileName( inputImageFile );
  try
  {
    reader->Update( );
  }
  catch( itk::ExceptionObject& err )
  {
    std::cout << "Exception object caught !" << std::endl; 
    std::cout << err << std::endl; 
    return( -1 );

  } // yrt

// Sacar centro de gravedad, matriz de inercia y masa total de la imagen.----------------------------------------------------------------------------
  TImageMoments::Pointer moments = TImageMoments::New();
  moments->SetImage( reader->GetOutput( ) );
  moments->Compute( );
  
// Se pone blanco el pixel que representa al centro de gravedad-------------------------------------------------------------------------------------
  TImage* img = reader->GetOutput( );
  TImage::Pointer output = TImage::New( );
  output->SetSpacing( img->GetSpacing( ) );
  output->SetOrigin( img->GetOrigin( ) );
  output->SetLargestPossibleRegion( img->GetLargestPossibleRegion( ) );
  output->SetRequestedRegion( img->GetRequestedRegion( ) );
  output->SetBufferedRegion( img->GetBufferedRegion( ) );
  output->Allocate( );

  TImage::SizeType size = img->GetRequestedRegion( ).GetSize( );

   unsigned int cx = (int)moments->GetCenter()[0];
   unsigned int cy = (int)moments->GetCenter()[1];

  for( unsigned int x = 0; x < size[ 0 ]; x++ )
  {
    for( unsigned int y = 0; y < size[ 1 ]; y++ )
    {
      TImage::IndexType idx;
      idx[ 0 ] = x;
      idx[ 1 ] = y;

      if( x == cx &&
          y == cy )
        output->SetPixel( idx, TPixel( 255  ) );
      else
	output->SetPixel(idx, img->GetPixel(idx) );     
	
    } // rof
  }

// Codigo para girar la imagen dependiendo el angulo del vector 1 de la matriz de inercia.-----------------------------------------------------------------
  float pos0 = moments->GetProperMatrix()[0][0];
  float pos1 = moments->GetProperMatrix()[1][0];
  
  const double scale = 1; 

 typedef itk::ResampleImageFilter<TImage, TImage >  FilterType;
 typedef itk::Similarity2DTransform< double >  TransformType;
 typedef itk::LinearInterpolateImageFunction<TImage, double >  InterpolatorType;

 FilterType::Pointer filter = FilterType::New();
 TransformType::Pointer transform = TransformType::New();
 InterpolatorType::Pointer interpolator = InterpolatorType::New();

 filter->SetInterpolator( interpolator );

 filter->SetDefaultPixelValue( 0 );
 reader->Update();

 const TImage::SpacingType&
   spacing = output->GetSpacing();
 const TImage::PointType&
   origin  = output->GetOrigin();
 TImage::SizeType imgsize =
     output->GetLargestPossibleRegion().GetSize();

 filter->SetOutputOrigin( origin );
 filter->SetOutputSpacing( spacing );
 filter->SetSize( imgsize );
 filter->SetInput( output );

 TransformType::InputPointType rotationCenter;
 rotationCenter[0] = moments->GetCenter( )[0];
 rotationCenter[1] = moments->GetCenter( )[1];
 transform->SetCenter( rotationCenter );

 //Ojo! es en radianes
 transform->SetAngle( calcularAnguloDeRotacion(pos0, pos1) );

 transform->SetScale( scale );
 TransformType::OutputVectorType translation;

 translation[0] =   0;
 translation[1] =   0;

 transform->SetTranslation( translation );
 filter->SetTransform( transform );

// Se corta la imagen justo por el centro de gravedad. --------------------------------------------------------------------------------
  
  typedef itk::RegionOfInterestImageFilter <TImage, TImage> CropImageFilterType;
 
  CropImageFilterType::Pointer cropFilterIzq = CropImageFilterType::New();
  CropImageFilterType::Pointer cropFilterDer = CropImageFilterType::New();
  cropFilterIzq->SetInput(filter->GetOutput());
  cropFilterIzq->SetInput(filter->GetOutput());
  
  ImageType::RegionType regionIzq;
  ImageType::RegionType regionDer;
  ImageType::SizeType   cropSizeIzq;
  ImageType::IndexType  indexIzq;
  ImageType::SizeType   cropSizeDer;
  ImageType::IndexType  indexDer;

  regionIzq=output->GetLargestPossibleRegion();  
  regionDer=output->GetLargestPossibleRegion();  

  indexIzq=regionIzq.GetIndex();
  cropSizeIzq=regionIzq.GetSize();

  indexDer=regionDer.GetIndex();
  cropSizeDer=regionDer.GetSize();

//Izquierda---------------------------------------------------------------------------------------------------------------------------------------------
  //x inicial imagen izquierda
  indexIzq.SetElement(0,0);
  //y inicial imagen izquierda
  indexIzq.SetElement(1,0);

  //extension a cortar en x imagen izquierda
  cropSizeIzq.SetElement(0,moments->GetCenter( )[0]);
  //extension a cortar en y - todo en este caso imagen izquierda
  cropSizeIzq.SetElement(1,size[1]);
  
  regionIzq.SetSize(cropSizeIzq);
  regionIzq.SetIndex(indexIzq);
    
  cropFilterIzq->SetInput(filter->GetOutput());
  cropFilterIzq->SetRegionOfInterest(regionIzq);
  cropFilterIzq->ReleaseDataFlagOn();
  cropFilterIzq->Update();

// Derecha---------------------------------------------------------------------------------------------------------------------------------------------
  //x inicial imagen derecha
  indexDer.SetElement(0,moments->GetCenter( )[0]);
  //y inicial imagen derecha
  indexDer.SetElement(1,0);

  //extension a cortar en x imagen derecha
  cropSizeDer.SetElement(0,size[0]-moments->GetCenter( )[0]);
  //extension a cortar en y - todo en este caso imagen derecha
  cropSizeDer.SetElement(1,size[1]);
  
  regionDer.SetSize(cropSizeDer);
  regionDer.SetIndex(indexDer);

  cropFilterDer->SetInput(filter->GetOutput());
  cropFilterDer->SetRegionOfInterest(regionDer);
  cropFilterDer->ReleaseDataFlagOn();

  cropFilterDer->Update();

//Izquierda!!!!************************************************************************************************************************************** 
// Sacar centro de gravedad, matriz de inercia y masa total de la imagen.----------------------------------------------------------------------------
  TImageMoments::Pointer izqmoments = TImageMoments::New();
  izqmoments->SetImage( cropFilterIzq->GetOutput() );
  izqmoments->Compute( );
  
// Se pone blanco el pixel que representa al centro de gravedad-------------------------------------------------------------------------------------
  TImage* izqimg = cropFilterIzq->GetOutput();
  TImage::Pointer izqoutput = TImage::New( );
  izqoutput->SetSpacing( izqimg->GetSpacing( ) );
  izqoutput->SetOrigin( izqimg->GetOrigin( ) );
  izqoutput->SetLargestPossibleRegion( izqimg->GetLargestPossibleRegion( ) );
  izqoutput->SetRequestedRegion( izqimg->GetRequestedRegion( ) );
  izqoutput->SetBufferedRegion( izqimg->GetBufferedRegion( ) );
  izqoutput->Allocate( );
  TImage::SizeType izqsize = izqimg->GetRequestedRegion( ).GetSize( );

   unsigned int izqcx = (int)izqmoments->GetCenter()[0];
   unsigned int izqcy = (int)izqmoments->GetCenter()[1];

  for( unsigned int izqx = 0; izqx < izqsize[ 0 ]; izqx++ )
  {
    for( unsigned int izqy = 0; izqy < izqsize[ 1 ]; izqy++ )
    {
      TImage::IndexType idx;
      idx[ 0 ] = izqx;
      idx[ 1 ] = izqy;

      if( izqx == izqcx  && izqy == izqcy )
        izqoutput->SetPixel( idx, TPixel( 255  ) );
      else
	izqoutput->SetPixel(idx, izqimg->GetPixel(idx) );     
	

    } // rof
 
 }


//Derecha!!!!************************************************************************************************************************************** 
// Sacar centro de gravedad, matriz de inercia y masa total de la imagen.----------------------------------------------------------------------------
  TImageMoments::Pointer dermoments = TImageMoments::New();
  dermoments->SetImage( cropFilterDer->GetOutput() );
  dermoments->Compute( );
  
// Se pone blanco el pixel que representa al centro de gravedad-------------------------------------------------------------------------------------
  TImage* derimg = cropFilterDer->GetOutput();
  TImage::Pointer deroutput = TImage::New( );
  deroutput->SetSpacing( derimg->GetSpacing( ) );
  deroutput->SetOrigin( derimg->GetOrigin( ) );
  deroutput->SetLargestPossibleRegion( derimg->GetLargestPossibleRegion( ) );
  deroutput->SetRequestedRegion( derimg->GetRequestedRegion( ) );
  deroutput->SetBufferedRegion( derimg->GetBufferedRegion( ) );
  deroutput->Allocate( );
  TImage::SizeType dersize = derimg->GetRequestedRegion( ).GetSize( );

   unsigned int dercx = (int)(dermoments->GetCenter()[0] - moments->GetCenter( )[0]);
   unsigned int dercy = (int)dermoments->GetCenter()[1];

  for( unsigned int derx = 0; derx < dersize[ 0 ]; derx++ )
  {
    for( unsigned int dery = 0; dery < dersize[ 1 ]; dery++ )
    {
      TImage::IndexType idx;
      idx[ 0 ] = derx;
      idx[ 1 ] = dery;

      if( derx == dercx &&
          dery == dercy )
        deroutput->SetPixel( idx, TPixel( 255) );
      else
	deroutput->SetPixel(idx, derimg->GetPixel(idx) );     
	



    } // rof
  }
/*
//Izquierda!!!!************************************************************************************************************************************** 
// Codigo para girar la imagen dependiendo el angulo del vector 1 de la matriz de inercia.-----------------------------------------------------------------
 float izqpos0 = izqmoments->GetProperMatrix()[0][1];
 float izqpos1 = izqmoments->GetProperMatrix()[1][1];

 FilterType::Pointer izqfilter = FilterType::New();
 TransformType::Pointer izqtransform = TransformType::New();
 InterpolatorType::Pointer izqinterpolator = InterpolatorType::New();

 izqfilter->SetInterpolator( izqinterpolator );

 izqfilter->SetDefaultPixelValue( 0 );
 reader->Update();

 const TImage::SpacingType&
   izqspacing = izqoutput->GetSpacing();
 const TImage::PointType&
   izqorigin  = izqoutput->GetOrigin();
 TImage::SizeType izqimgsize =
   izqoutput->GetLargestPossibleRegion().GetSize();

 izqfilter->SetOutputOrigin( izqorigin );
 izqfilter->SetOutputSpacing( izqspacing );
 izqfilter->SetSize( izqimgsize );
 izqfilter->SetInput( izqoutput );

 rotationCenter[0] = izqmoments->GetCenter( )[0];
 rotationCenter[1] = izqmoments->GetCenter( )[1];
 izqtransform->SetCenter( rotationCenter );

 //Ojo! es en radianes
 izqtransform->SetAngle( calcularAnguloDeRotacion(izqpos0, izqpos1) );

 izqtransform->SetScale( scale );
 
 translation[0] =   0;
 translation[1] =   0;

 izqtransform->SetTranslation( translation );
 izqfilter->SetTransform( izqtransform );

//Derecha!!!!************************************************************************************************************************************** 
// Codigo para girar la imagen dependiendo el angulo del vector 1 de la matriz de inercia.-----------------------------------------------------------------
 float derpos0 = dermoments->GetProperMatrix()[0][1];
 float derpos1 = dermoments->GetProperMatrix()[1][1];

 FilterType::Pointer derfilter = FilterType::New();
 TransformType::Pointer dertransform = TransformType::New();
 InterpolatorType::Pointer derinterpolator = InterpolatorType::New();

 derfilter->SetInterpolator( derinterpolator );

 derfilter->SetDefaultPixelValue( 0 );
 reader->Update();

 const TImage::SpacingType&
   derspacing = deroutput->GetSpacing();
 const TImage::PointType&
   derorigin  = deroutput->GetOrigin();
 TImage::SizeType derimgsize =
   deroutput->GetLargestPossibleRegion().GetSize();

 derfilter->SetOutputOrigin( derorigin );
 derfilter->SetOutputSpacing( derspacing );
 derfilter->SetSize( derimgsize );
 derfilter->SetInput( deroutput );

 rotationCenter[0] = dermoments->GetCenter( )[0];
 rotationCenter[1] = dermoments->GetCenter( )[1];
 dertransform->SetCenter( rotationCenter );

 //Ojo! es en radianes
 dertransform->SetAngle( calcularAnguloDeRotacion(derpos0, derpos1) );

 dertransform->SetScale( scale );
 
 translation[0] =   0;
 translation[1] =   0;

 dertransform->SetTranslation( translation );
 derfilter->SetTransform( dertransform );
*/

//Izquierda********************************************************************************************************************************************
//Se crea los ejes x y con origen en el centro de gravedad.--------------------------------------------------------------------------------------------
 
//TImage* izqejeoutput = izqfilter->GetOutput();
 float izqpos0 = izqmoments->GetProperMatrix()[0][0];
 float izqpos1 = izqmoments->GetProperMatrix()[1][0]; 
 float izqpos2 = izqmoments->GetProperMatrix()[0][1];
 float izqpos3 = izqmoments->GetProperMatrix()[1][1]; 

 float izqangulo = calcularAnguloDeRotacion(izqpos0, izqpos1);
 float izqangulo1 = calcularAnguloDeRotacion(izqpos2, izqpos3);
 int x0 = (int) izqmoments->GetCenter()[0];
 int y0 = (int) izqmoments->GetCenter()[1];

 int y1 = 0;
 int x1 = (int)abs(((atan (izqangulo1) * (y0 - y1)) + x0));
 
drawLine(izqoutput,x1, y1, x0, y0);

 y1 = izqoutput->GetLargestPossibleRegion().GetSize()[1];
 x1 = (int)abs(((tan (izqangulo1) * (y1 - y0)) - x0));

drawLine(izqoutput,x0, y0, x1, y1);

//Derecha********************************************************************************************************************************************
//Se crea los ejes x y con origen en el centro de gravedad.--------------------------------------------------------------------------------------------

 //drawLine(TImage img, int x0, int y0, int x1, int y1)
 //drawLine(TImage img, int x0, int y0, int x1, int y1)

// Se imprimen las dos imagenes.-----------------------------------------------------------------------------------------------------------------------
  TWriter::Pointer writerIzq = TWriter::New( );
  writerIzq->SetFileName( "izq" + outputImageFile );
 
  TWriter::Pointer writerDer = TWriter::New( );
  writerDer->SetFileName( "der" + outputImageFile );

  writerIzq->SetInput(izqoutput);
  writerDer->SetInput(deroutput);
  try
  {
    writerIzq->Update( );
    writerDer->Update( );
  }
  catch( itk::ExceptionObject &err )
  {
    std::cout << "Exception object caught !" << std::endl; 
    std::cout << err << std::endl; 
    return( -1 );
  
  return( 0 );
}
std::cout
    << "Mass total: " << std::endl
    << moments->GetMass( ) << std::endl
    << "Center of gravity: " << std::endl
    << moments->GetCenter( ) << std::endl
    << "Inertia matrix: " << std::endl
    << moments->GetProperMatrix( )<< std::endl
    << "Mass izq: " << std::endl
    << izqmoments->GetMass( ) << std::endl
    << "Center of gravity: " << std::endl
    << izqmoments->GetCenter( ) << std::endl
    << "Inertia matrix: " << std::endl
    << izqmoments->GetProperMatrix( )<< std::endl
    << "Mass der: " << std::endl
    << dermoments->GetMass( ) << std::endl
    << "Center of gravity: " << std::endl
    << dermoments->GetCenter( ) << std::endl
    << "Inertia matrix: " << std::endl
    << dermoments->GetProperMatrix( )<< std::endl;
}


// eof - $RCSfile$
